#include "PidController.h"
#include "Arduino.h"

PidController::PidController(float kp, float ki, float kd)
{
    reset();
    update_pid(kp, ki, kd);
}

float PidController::update(float current)
{
    // 计算误差及其变化率
    float error = target_ - current;
    derror_ = error_last_ - error;
    error_last_ = error;

    // 计算积分项并进行积分限制
    error_sum_ += error;
    if (error_sum_ > intergral_up_)
    {
        /* code */
        error_sum_ = intergral_up_;
    }
    if (error_sum_ < -1 * intergral_up_)
    {
        /* code */
        error_sum_ = -1 * intergral_up_;
    }

    float output = kp_ * error + ki_ * error_sum_ + kd_ * derror_;

    if (output>out_max_)
    {
        /* code */
        output=out_max_;
    }
    if (output<out_min_)
    {
        /* code */
        output=out_min_;
    }

    return output;
}

void PidController::update_target(float target){
    target_ = target;
}

void PidController::update_pid(float kp, float ki, float kd){
    reset(); // 重置控制器状态
    kp_ = kp;
    ki_ = ki;
    kd_ = kd;
}

void PidController::reset(){
    target_ = 0.0f;
    out_min_ = 0.0f;
    out_max_ = 0.0f;
    kp_ = 0.0f;
    ki_ = 0.0f;
    kd_ = 0.0f;
    error_sum_ = 0.0f;
    derror_ = 0.0f;
    error_last_ = 0.0f;
}

void PidController::out_limit(float out_min, float out_max){
    out_min_ = out_min;
    out_max_ = out_max;
}